/*
 * planner_lookahead.c
 *
 * Copyright 2011 Pieter Agten
 *
 * This file is part of Yarf.
 *
 * Yarf is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Yarf is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Yarf.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file   planner_lookahead.c
 * @author Pieter Agten (pieter.agten@gmail.com)
 * @author Simen Svale Skogsrud (author of Grbl)
 * @author Sonny Jeon (author of fork of Grbl)
 * @date   5 nov 2011
 * @brief  The lookahead component tries to keep the speed at which movements
 *         are made as high as possible. That is, it tries to raise the entry
 *         and exit speeds of blocks, while at the same time preventing jerky
 *         movement. The code is based on chamnit's fork of Grbl 
 *         (https://github.com/chamnit/grbl).
 */

#include "planner_lookahead.h"

#include "yarf.h"
#include "planner.h"
#include "planner_queue.h"
#include "util/math.h"

#include <stdint.h>
#include <string.h>


#if PLANNER_LOOKAHEAD
/**
 * Information about the last block processed by the lookahead component, i.e.
 * the last block added to the planning queue.
 */
static struct {
  float direction[NUM_AXES]; /**< The four-dimensional unit direction vector (containing the direction cosines for each axis) of the previous block.*/
  float plateau_speed; /**< The plateau speed (the maximum speed) of the previous block in mm/min. */
} previous_block_info;





/**
 * Returns the highest speed that will still allow the machine to reach a 
 * given target speed within a given distance, without decelerating faster than
 * the configured acceleration.
 *
 * @param target_speed the speed that needs to be reached after \a distance_mm
 * @param distance_mm  the distance after which the target speed must be reached
 * @return The highest speed that will still allow the machine to reach a 
 *         given target speed within a given distance, without decelerating
 *         faster than the configured acceleration.
 *
@verbatim
             speed ^
                   |     +-----+  <-- return value
                   |          : \                                
                   |          :  \
                   |          :   +  <-- target speed                               
                   |          :   |
                   |   +------:---+
                   |          :   :
                   |            ^----- distance_mm
                   |______________________
                      distance traveled --> 
@endverbatim
 *
 * You can also view this function as returning the maximum obtainable speed,
 * within a given distance, starting from \a target_speed and without exceeding
 * the configured acceleration.
 *
 * @see ACCELERATION 
 */
static speed_t 
max_allowable_speed(speed_t target_speed, float distance_mm)
{
  return sqrt(lsquare(target_speed)+2*ACCELERATION*distance_mm);
}



/**
 * The kernel for the reverse pass of the lookahead algorithm.
 *
 * @param current  pointer to the block to which to apply this kernel
 * @param next     pointer to the block that will be executed by the block
 *                 handler after the block referred to by \a current, i.e. the
 *                 block processed previously by this kernel.
 *
 * This kernel tries to raise the entry speed of the block pointed to by \a
 * current. It will raise the entry speed as high as possible, while still
 * making sure the machine has time enough to be able to decelerate down to the
 * entry speed of the next block. Also, the entry speed will never be raised
 * higher than the block's \a max_entry_speed, which was calculated earlier in
 * the \a plan_lookahead method, based on the angle between this block and the
 * previous block (the block that is executed by the block handler before the
 * block pointed to by \a current).
 */
static void 
reverse_pass_kernel(block_t *current, block_t *next)
{
  if (! current) { return; }
  
  if (next) { 
    // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
    // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and 
    // check for maximum allowable speed reductions to ensure maximum possible planned speed.
    if (current->entry_speed < current->max_entry_speed) {
      if (current->plateau_speed_always_reachable || current->max_entry_speed < next->entry_speed) {
        current->entry_speed = current->max_entry_speed;
      } else {
        current->entry_speed = MIN(current->max_entry_speed, max_allowable_speed(next->entry_speed,current->distance_mm));
      }
      current->recalculate = true;    
    }
  } // Skip last block. Already initialized and set for recalculation.
}


/**
 * Calls the reverse pass kernel for each element of the planning queue, in
 * reverse order. That is, the block that was added last to the planning queue
 * will be handled first and the block that will be executed first by the block
 * handler (the block at the tail of the queue) is handled last. The reverse
 * pass is performed before the forward pass.
 */
static void
reverse_pass(void)
{
  uint8_t block_index = plan_q_head_index();
  block_t *block[2] = {NULL, NULL};
  while(block_index != plan_q_tail_index()) {
    block_index = plan_q_prev_index(block_index);
    block[1] = block[0];
    block[0] = plan_q_get(block_index);
    reverse_pass_kernel(block[0], block[1]);
  }
}


/**
 * The kernel for the forward pass of the lookahead algorithm.
 *
 * @param previous pointer to the block that will be executed by the block
 *                 handler before the block referred to by \a current, i.e. the
 *                 block processed previously by this kernel.
 * @param current  pointer to the block to which to apply this kernel
 *
 * Lowers the entry speed of the block pointed to by \a current, if necessary.
 * The entry speed needs to be lowered if the previous block is slow and too
 * short for the machine to be able to accelerate up to the entry speed of the
 * current block.
 */
static void
forward_pass_kernel(block_t *previous, block_t *current)
{
  if (! previous) { return; }  // Begin planning after buffer_tail
  
  // If the previous block is an acceleration block, but it is not long enough to complete the
  // full speed change within the block, we need to adjust the entry speed accordingly. Entry
  // speeds have already been reset, maximized, and reverse planned by reverse planner.
  // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.  
  if (! previous->plateau_speed_always_reachable) {
    if (previous->entry_speed < current->entry_speed) {
      speed_t max_obtainable_speed = max_allowable_speed(previous->entry_speed,previous->distance_mm);

      // Check for junction speed change
      if (max_obtainable_speed < current->entry_speed) {
        current->entry_speed = max_obtainable_speed;
        current->recalculate = true;
      }
    }
  }
}




/**
 * Calls the forward pass kernel for each element of the planning queue, in
 * forward order. That is, the block that will be executed first by the block
 * handler (the block at the tail of the queue) is handled first and the block
 * that was added last to the planning queue will be handled last. The forward
 * pass is performed after the reverse pass.
 */
static void 
forward_pass(void)
{
  uint8_t block_index = plan_q_tail_index();
  block_t *block[2] = {NULL, NULL};
  while(block_index != plan_q_head_index()) {
    block[0] = block[1];
    block[1] = plan_q_get(block_index);
    forward_pass_kernel(block[0],block[1]);
    block_index = plan_q_next_index(block_index);
  }
}

                                                                          
/**
 * Calls the \a plan_calculate_trapezoid method from the planner component for
 * each block in the planning queue for which the entry or exit speed was 
 * changed by the reverse or forward passes of the lookahead algorithm. The
 * exit speed of a block is set equal to entry speed of the next block in the
 * queue (the block that will be executed next by the block handler).
 *
@verbatim
             speed ^
                   |
                   |     +-------+
                   |    /         \
     entry speed --|-> +           \
                   |   |            + <-- exit speed = 
                   |   +------------+     next->entry_speed
                   |____________________________
                                        time -->
@endverbatim
 */
static void
recalculate_trapezoids(void)
{
  uint8_t block_index = plan_q_tail_index();
  block_t *current;
  block_t *next = NULL;
  
  while(block_index != plan_q_head_index()) {
    current = next;
    next = plan_q_get(block_index);
    if (current) {
      // Recalculate if current block entry or exit junction speed has changed.
      if (current->recalculate || next->recalculate) {
        plan_calculate_trapezoid(current, current->entry_speed, next->entry_speed);
        current->recalculate = false;
      }
    }
    block_index = plan_q_next_index(block_index);
  }
  // Last/newest block in buffer. Exit speed is set with START_FEED_RATE. Always recalculated.
  plan_calculate_trapezoid(next, next->entry_speed, MIN(next->plateau_speed, START_FEED_RATE));
  next->recalculate = false;
}


/**
 * Recalculates the motion plan for the entire queue of blocks by first calling
 * the reverse pass, then calling the forward pass and finally calling the \a
 * recalculate_trapezoids method, to recalculate the acceleration parameters
 * based on the changes to the entry speeds performed by the reverse and
 * forward passes.
 */
static inline void 
recalculate_motion_plan(void) {     
  reverse_pass();
  forward_pass();
  recalculate_trapezoids();
}


void
plan_lookahead_init()
{
  previous_block_info.plateau_speed = 0;
}


void
plan_lookahead()
{
  block_t *b = plan_q_get(plan_q_prev_index(plan_q_head_index()));
  // Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
  // Let a circle be tangent to both previous and current path line segments, where the junction 
  // deviation is defined as the distance from the junction to the closest edge of the circle, 
  // colinear with the circle center. The circular segment joining the two paths represents the 
  // path of centripetal acceleration. Solve for max velocity based on max acceleration about the
  // radius of the circle, defined indirectly by junction deviation. This may be also viewed as 
  // path width or max_jerk in the previous grbl version. This approach does not actually deviate 
  // from path, but used as a robust way to compute cornering speeds, as it takes into account the
  // nonlinearities of both the junction angle and junction velocity.
  const speed_t start_feed_rate = MIN(b->plateau_speed, START_FEED_RATE);
  speed_t max_junction_speed = start_feed_rate; // Set default conservative max junction speed

  // Skip first block
  if (plan_q_size() >= 2) {
    // Make sure the junction speed will not be higher than the previous plateau speed.
    max_junction_speed = MIN(max_junction_speed, previous_block_info.plateau_speed);
    // Compute cosine of angle between previous and current path.
    // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
    float cos_theta = -(previous_block_info.direction[X_AXIS] * b->direction[X_AXIS] +
                        previous_block_info.direction[Y_AXIS] * b->direction[Y_AXIS] +
                        previous_block_info.direction[Z_AXIS] * b->direction[Z_AXIS] +
                        previous_block_info.direction[E_AXIS] * b->direction[E_AXIS]) ;

    // Skip and use default max junction speed for 0 degree acute junction.
    if (cos_theta < 0.95) { 
      // Skip and avoid divide by zero for straight junctions at 180 degrees.
      if (cos_theta > -0.95) { // -0.95 < cos_theta < 0.95
        // Compute maximum junction velocity based on maximum acceleration and junction deviation
        float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
        max_junction_speed = MIN(max_junction_speed, sqrt(ACCELERATION * JUNCTION_DEVIATION * sin_theta_d2/(1.0-sin_theta_d2)) );
      } // else { very unsharp corner }
    } // else { very sharp corner } 
  }
  b->max_entry_speed = max_junction_speed;

  // Calculate the maximum allowable speed to still be able to decelerate to START_FEED_RATE over
  // the length of the block.
  speed_t max_allowable_speed_ = max_allowable_speed(start_feed_rate,b->distance_mm);
  b->entry_speed = MIN(max_junction_speed, max_allowable_speed_);

  // Initialize planner efficiency flags
  // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
  // If a block can decelerate from nominal speed to minimum speed within the length of the block, then
  // the current block and next block junction speeds are guaranteed to always be at their maximum
  // junction speeds in deceleration and acceleration, respectively. This is due to how the current
  // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
  // the reverse and forward planners, the corresponding block junction speed will always be at the
  // the maximum junction speed and may always be ignored for any speed reduction checks.
  b->plateau_speed_always_reachable = (b->plateau_speed <= max_allowable_speed_);
  b->recalculate = true; // Always calculate trapezoid for new block
  
  // Update previous path unit_vector and nominal speed
  memcpy(previous_block_info.direction, b->direction, sizeof(b->direction));
  previous_block_info.plateau_speed = b->plateau_speed;

  recalculate_motion_plan();
}

#endif //PLANNER_LOOKAHEAD

